#pragma once

#include <iostream>
#include <cmath>


#include "MP_DefineData.h"

double CarPointDistance(CAR_POINT p0, CAR_POINT p1);
double GeoPointDistance(GEO_POINT point0, GEO_POINT point1);
void Car2Geo(double lon0, double lat0, double x, double y, double lonLat1[2]);
void Geo2Car(double lon0, double lat0, double lon1, double lat1, double xy[2]);
double GeoDirectionAngle(double lon0, double lat0, double lon1, double lat1);
void PosiYawRotateMatrix2D(double yaw, double rotMat[2][2]);
double CounterclockwiseAngle(CAR_POINT point0, CAR_POINT point1);
double VectorCross(double vec1[2], double vec2[2]);